package pprz.data_components;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.HashMap;

import net.jscience.math.MathFP;
import pprz.Link;
import pprz.data_components.events.DownlinkMessageEvent;
import pprz.data_components.parsers.MessageParser.MessageFormat;

public class LogPlayerMsgProtocol extends MsgProtocol {
	
	protected Message message; 
	protected MessageFormat format;
	protected Link link;
	HashMap<String, MessageFormat>  message_formats;
	
	
	public LogPlayerMsgProtocol(Link link)
	{
		super();
		message=new Message();
		this.link=link;
		message_formats=new HashMap<String, MessageFormat> ();
	}
	
	//decode byte array to an aircraft info structure, wrap it in a message and send to GUI
	public void decode(byte[] cc) 
	{
		if (cc.length<2) return; //check length
		
		String s=new String(cc);
		String[] fields=s.split("\\s");
		//System.out.println(s);
		int id=Integer.decode(fields[1]); //sender id
		message._ac=(short)id;
		if (!message_formats.containsKey(fields[2]))
		{
			for (MessageFormat f : link.getMessageFormats().values())
			{
				if (!message_formats.containsKey(f.name))
					message_formats.put(f.name, f);
			}
		}
		
		if (message_formats.containsKey(fields[2])) format=message_formats.get(fields[2]);
		else return;
		
		int msgid=format.mId; //msg id
		//Fill in known Aircraft structures
		Aircraft a;
		if (!link.getAircraftList().containsKey(id)) {
			link.getAircraftList().put(id, new Aircraft(id,link.getAircraftList().size()));
		}
		a=link.getAircraftList().get(id);
		a.lastMSG=0;
		
		if (cc.length > 2) {
			ByteBuffer b = ByteBuffer.wrap(cc, 2, cc.length - 2);
			b.order(ByteOrder.LITTLE_ENDIAN);

			format=link.getMessageFormats().get(0*0x100000000l+msgid);
			if (format.noName || format.noClass || format.noId || 
				format.noTypeField>0 || format.noNameField>0) return;
			//else format is ok --->
			message.setFieldsLength(format.fields.size());
			Long[] fld;
			for (int i=0;i<message.getFields().size();i++)
			{
				switch (format.fields.get(i).type)
				{
				case 0://int8,uint8
				case 1:
					fld=new Long[1];
					fld[0]=MathFP.toFP(Byte.decode(fields[i+3]));
					message.setField(i, fld);
					break;
				case 2://int16
				case 3://uint16
					fld=new Long[1];
					fld[0]=MathFP.toFP(Short.decode(fields[i+3]));
					message.setField(i, fld);
					break;
				case 4://int32
				case 5://uint32
					fld=new Long[1];
					fld[0]=MathFP.toFP(Integer.decode(fields[i+3]));
					message.setField(i, fld);
					break;
				case 6://float
					fld=new Long[1];
					fld[0]=MathFP.toFP((long)(Float.parseFloat(fields[i+3])*1E8));
					message.setField(i, fld);
					break;
				case 7://string
					break;
				case 8: // uint16[]
				case 9: // int16[]
					String[] ss=fields[3].split(",");
					int n=ss.length; // CORRECT THIS / clean format
					fld=new Long[n];
					for (int j=0;j<n;j++)
					{
						fld[j]=MathFP.toFP(Short.decode(ss[j]));
					}
					message.setField(i, fld);
					break;
				default: 
					System.out.println(this.getClass().toString()+": invalid message field type.");
				}
			}
			
			a.lastMSG=msgid;		
			switch (msgid) {
				case 6: {
					//set roll, heading, pitch
					a.set_roll(Short.decode(fields[3]));
					//a.set_yaw(Short.decode(fields[4]));
					a.set_pitch(Short.decode(fields[5]));
					break;
				}
				case 8: {
					a.set_gps_mode(Byte.decode(fields[3]));
					a.set_gps_utm_east(Integer.decode(fields[4])/100.f);
					a.set_gps_utm_north(Integer.decode(fields[5])/100.f);
					a.set_gps_course(Short.decode(fields[6])/10.f);
					a.set_yaw(a.get_gps_course()); // we dont have yaw sensor, assume yaw=heading
					a.set_gps_alt(Integer.decode(fields[7])/100.f);
					a.set_gps_vel(Short.decode(fields[8])/100.f);
					a.set_gps_climb(Short.decode(fields[9])/100.f);
					a.set_gps_itow(Integer.decode(fields[10]));
					a.set_gps_utm_zone(Byte.decode(fields[11]));
					a.set_gps_nb_err(Byte.decode(fields[12]));					
					a.updateTraj();
					break;				
				}
				case 10: {
					a.set_cur_block(Byte.decode(fields[3]));
					a.set_cur_stage(Byte.decode(fields[4]));
					
					a.set_desired_course(Short.decode(fields[6])/100.f);
					a.set_dist2_wp(Float.parseFloat(fields[7]));
					a.set_dist2_home(Float.parseFloat(fields[8]));
					break;
				}
				case 11: {
					a.set_ap_mode(Byte.decode(fields[3]));
					a.set_ap_gaz(Byte.decode(fields[4]));
					a.set_ap_lateral(Byte.decode(fields[5]));
					a.set_ap_horizontal(Byte.decode(fields[6]));
					a.set_if_calib_mode(Byte.decode(fields[7]));
					a.set_mcu1_status(Byte.decode(fields[8]));
					a.set_lls_calib(Byte.decode(fields[9]));
					break;				
				}
				case 12: {
					a.set_throttle(Short.decode(fields[3])/9600.f);
					a.set_voltage(Byte.decode(fields[4])/10.f); 
					a.set_flight_time(Short.decode(fields[5]));
					a.set_kill_auto_throttle(Byte.decode(fields[6]));
					a.set_block_time(Short.decode(fields[7]));
					a.set_stage_time(Short.decode(fields[8]));
					a.set_energy(Short.decode(fields[9]));
					break;				
				}
				case 16: {
					a.set_des_roll(Float.parseFloat(fields[3]));
					a.set_des_pitch(Float.parseFloat(fields[4]));
					a.set_des_x(Float.parseFloat(fields[5]));
					a.set_des_y(Float.parseFloat(fields[6]));
					a.set_des_altitude(Float.parseFloat(fields[7]));
					a.set_des_climb(Float.parseFloat(fields[8]));
					break;
				}
					
				case 23: {
					a.set_rx_bytes_rate(Float.parseFloat(fields[7]));
					a.set_rx_msgs_rate(Float.parseFloat(fields[8]));
					break;				
				}
				default: {
					break;
				}
			}
			message.setClass((byte)0);
			message.setId(msgid);
			message.setTime(System.currentTimeMillis());
//			if (a.state==Aircraft.NOTHING) { // not aircraft message, pass it on
				message.setPayload(cc); 
//			}
//			else { // aircraft message, appropriate fields are filled
				message.setPayload(a); 
//			}			
			fireDownlinkMessageEvent(new DownlinkMessageEvent(this,message));
		}
		else return; //message is too short, skip it
	}	
}
